#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/follow_waypoints.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <iostream>
#include <vector>
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"


using FollowWaypoints = nav2_msgs::action::FollowWaypoints;
using GoalHandleFollowWaypoints = rclcpp_action::ClientGoalHandle<FollowWaypoints>;

class WaypointFollower : public rclcpp::Node
{
public:
  WaypointFollower() : Node("waypoint_follower")
  {
    // 创建 FollowWaypoints 动作客户端
    client_ptr_ = rclcpp_action::create_client<FollowWaypoints>(
      this, "follow_waypoints");
    
              // 创建初始位姿发布器
    initial_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "initialpose", 10);

    // 等待动作服务器启动
    while (!client_ptr_->wait_for_action_server(std::chrono::seconds(1))) {
      RCLCPP_INFO(this->get_logger(), "等待 FollowWaypoints 动作服务器...");
    }
    RCLCPP_INFO(this->get_logger(), "动作服务器已连接");
            // 初始化位姿为原点 (0,0,0)
    initialize_pose_at_origin();
  }

   // 初始化机器人位姿为原点
  void initialize_pose_at_origin()
    {
        auto init_pose = geometry_msgs::msg::PoseWithCovarianceStamped();
        init_pose.header.frame_id = "map";
        init_pose.header.stamp = this->now();
        
        // 设置位姿为原点 (x=0, y=0, z=0, 无旋转)
        init_pose.pose.pose.position.x = 0.0;
        init_pose.pose.pose.position.y = 0.0;
        init_pose.pose.pose.position.z = 0.0;
        init_pose.pose.pose.orientation.x = 0.0;
        init_pose.pose.pose.orientation.y = 0.0;
        init_pose.pose.pose.orientation.z = 0.0;
        init_pose.pose.pose.orientation.w = 1.0;
        
        // 设置协方差矩阵（对角元素表示不确定性）
        for (size_t i = 0; i < 36; ++i) init_pose.pose.covariance[i] = 0.0;
        init_pose.pose.covariance[0] = 0.25;  // x方向不确定性
        init_pose.pose.covariance[7] = 0.25;  // y方向不确定性
        init_pose.pose.covariance[35] = 0.06853891945200942;  // 朝向不确定性
        
        // 发布初始位姿（需多次发布确保被接收）
        RCLCPP_INFO(this->get_logger(), "初始化机器人位姿为原点 (0,0,0)");
        for (int i = 0; i < 5; ++i) {
            initial_pose_pub_->publish(init_pose);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
        
        // 等待一小段时间让系统处理初始位姿
        RCLCPP_INFO(this->get_logger(), "位姿初始化完成，等待系统处理...");
        rclcpp::sleep_for(std::chrono::seconds(2));
    }
  // 发送航点列表
  void send_waypoints(const std::vector<geometry_msgs::msg::PoseStamped> & waypoints)
  {
    // 构造动作目标
    auto goal_msg = FollowWaypoints::Goal();
    goal_msg.poses = waypoints;
    
    RCLCPP_INFO(this->get_logger(), "发送 %lu 个航点", waypoints.size());
    
    // 设置发送选项（包含反馈和结果回调）
    auto send_goal_options = rclcpp_action::Client<FollowWaypoints>::SendGoalOptions();
    send_goal_options.feedback_callback = 
      [this](GoalHandleFollowWaypoints::SharedPtr, 
             const std::shared_ptr<const FollowWaypoints::Feedback> feedback) {
        RCLCPP_INFO(this->get_logger(), "当前执行第 %d 个航点", feedback->current_waypoint + 1);
      };
    
    send_goal_options.result_callback = 
      [this](const GoalHandleFollowWaypoints::WrappedResult & result) {
        switch (result.code) {
          case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "所有航点导航成功！");
            break;
        //   case rclcpp_action::ResultCode::ABORTED:
        //     RCLCPP_ERROR(this->get_logger(), "导航被中止，失败原因：%s（错误码：%d）",
        //        error_str.c_str(), static_cast<int>(result.result->error_code));
        //     break;
          case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_WARN(this->get_logger(), "导航已取消");
            break;
          default:
            RCLCPP_ERROR(this->get_logger(), "未知导航结果");
            break;
        }
        task_completed_ = true;
      };
    
    // 发送目标
    auto goal_handle_future = client_ptr_->async_send_goal(goal_msg, send_goal_options);
    
    // 等待目标被接受
    if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), goal_handle_future) !=
        rclcpp::FutureReturnCode::SUCCESS) {
      RCLCPP_ERROR(this->get_logger(), "发送目标失败");
      task_completed_ = true;
    }
  }

  // 检查任务是否完成
  bool is_task_completed() const { return task_completed_; }

private:
  rclcpp_action::Client<FollowWaypoints>::SharedPtr client_ptr_;
  rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr initial_pose_pub_;
  bool task_completed_{false};
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<WaypointFollower>();

  while (rclcpp::ok()) {
    // 清空航点列表
    std::vector<geometry_msgs::msg::PoseStamped> waypoints;
    
    // 输入航点数量
    int num_waypoints;
    std::cout << "\n请输入航点的数量: ";
    std::cin >> num_waypoints;
    std::cin.ignore();  // 清除输入缓冲区的换行符
    
    // 循环输入每个航点
    for (int i = 0; i < num_waypoints; ++i) {
      geometry_msgs::msg::PoseStamped pose;
      pose.header.frame_id = "map";
      pose.header.stamp = node->get_clock()->now();
      
      double x, y;
      std::cout << "\n请输入第 " << i+1 << " 个航点的坐标:" << std::endl;
      std::cout << "x: ";
      std::cin >> x;
      std::cout << "y: ";
      std::cin >> y;
      std::cin.ignore();  // 清除输入缓冲区的换行符
      
      // 设置航点坐标
      pose.pose.position.x = x;
      pose.pose.position.y = y;
      pose.pose.orientation.w = 1.0;  // 无旋转
      
      // 添加到航点列表
      waypoints.push_back(pose);
      std::cout << "已添加航点 " << i+1 << ": (" << x << ", " << y << ")" << std::endl;
    }
    
    // 打印所有航点（验证是否正确）
    std::cout << "\n最终航点列表:" << std::endl;
    for (size_t i = 0; i < waypoints.size(); ++i) {
      const auto & pose = waypoints[i];
      std::cout << "航点 " << i+1 << ": (" 
                << pose.pose.position.x << ", " 
                << pose.pose.position.y << ")" << std::endl;
    }
    
    // 重置任务状态并发送航点
    node->is_task_completed();  // 重置状态
    node->send_waypoints(waypoints);
    
    // 等待导航完成
    while (rclcpp::ok() && !node->is_task_completed()) {
      rclcpp::spin_some(node);
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
  }

  rclcpp::shutdown();
  return 0;
}
